#include "LX_FC_Fun.h"
#include "LX_FC_State.h"
#include "ANO_DT_LX.h"

/*==========================================================================
 * 描述    ：凌霄飞控自定义程序
 ===========================================================================*/

//////////////////////////////////////////////////////////////////////
//功能函数
//////////////////////////////////////////////////////////////////////

//程序上升
u8 Vertical_Move_UP(u16 height_cm,u16 speed_cm_s)
{
	//
	if (dt.wait_ck == 0) //没有其他等待校验的CMD时才发送本CMD
	{
		//按协议发送指令
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X02;
		dt.cmd_send.CMD[1] = 0X01;
		dt.cmd_send.CMD[2] = BYTE0(height_cm);//升高 cm
		dt.cmd_send.CMD[3] = BYTE1(height_cm);
		dt.cmd_send.CMD[4] = BYTE0(speed_cm_s);//速度 cm/s 15better
		dt.cmd_send.CMD[5] = BYTE1(speed_cm_s);
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}
//程序下降
u8 Vertical_Move_DOWN(u16 height_cm,u16 speed_cm_s)
 {
	//
	if (dt.wait_ck == 0) //没有其他等待校验的CMD时才发送本CMD
	{
		//按协议发送指令
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X02;
		dt.cmd_send.CMD[1] = 0X02;
		dt.cmd_send.CMD[2] = BYTE0(height_cm);//降低 cm
		dt.cmd_send.CMD[3] = BYTE1(height_cm);
		dt.cmd_send.CMD[4] = BYTE0(speed_cm_s);//速度 cm/s 15better
		dt.cmd_send.CMD[5] = BYTE1(speed_cm_s);
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}
 
//程序左旋
u8 Turn_left(u16 turning_degree_deg,u16 deg_velocity_degps)
{
	//
	if (dt.wait_ck == 0) //没有其他等待校验的CMD时才发送本CMD
	{
		//按协议发送指令
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X02;
		dt.cmd_send.CMD[1] = 0X07;		                 //左旋
		dt.cmd_send.CMD[2] = BYTE0(turning_degree_deg);//转动角度 deg
		dt.cmd_send.CMD[3] = BYTE1(turning_degree_deg);
		dt.cmd_send.CMD[4] = BYTE0(deg_velocity_degps);//速度 deg/s
		dt.cmd_send.CMD[5] = BYTE1(deg_velocity_degps);
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}

//程序右旋
u8 Turn_right(u16 turning_degree_deg,u16 deg_velocity_degps)
{
	//
	if (dt.wait_ck == 0) //没有其他等待校验的CMD时才发送本CMD
	{
		//按协议发送指令
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X02;
		dt.cmd_send.CMD[1] = 0X08;		                 //右旋
		dt.cmd_send.CMD[2] = BYTE0(turning_degree_deg);//转动角度 deg
		dt.cmd_send.CMD[3] = BYTE1(turning_degree_deg);
		dt.cmd_send.CMD[4] = BYTE0(deg_velocity_degps);//速度 deg/s
		dt.cmd_send.CMD[5] = BYTE1(deg_velocity_degps);
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}

//程序左旋绕杆用不了
u8 Cycling_Move_left(u16 radius_cm,u16 velocity_cmps)
{
	//
	u16 distance_cm = (radius_cm * 3.14);
	u16 hori_degree_deg = 0;
	u16 turning_degree_deg = 180;
	u16 deg_velocity_degps = ((180 * velocity_cmps)/ distance_cm);
	if (dt.wait_ck == 0) //没有其他等待校验的CMD时才发送本CMD
	{
		//按协议发送指令
		//左旋
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X02;
		dt.cmd_send.CMD[1] = 0X07;
		dt.cmd_send.CMD[2] = BYTE0(turning_degree_deg);//转180度
		dt.cmd_send.CMD[3] = BYTE1(turning_degree_deg);
		dt.cmd_send.CMD[4] = BYTE0(deg_velocity_degps);//速度 cmps 15better
		dt.cmd_send.CMD[5] = BYTE1(deg_velocity_degps);
		CMD_Send(0xff, &dt.cmd_send);
		//平移
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X02;
		dt.cmd_send.CMD[1] = 0X03;
		//
		dt.cmd_send.CMD[2] = BYTE0(distance_cm);
		dt.cmd_send.CMD[3] = BYTE1(distance_cm);
		dt.cmd_send.CMD[4] = BYTE0(velocity_cmps);
		dt.cmd_send.CMD[5] = BYTE1(velocity_cmps);
		dt.cmd_send.CMD[6] = BYTE0(hori_degree_deg);
		dt.cmd_send.CMD[7] = BYTE1(hori_degree_deg);
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}

//程序右旋绕杆用不了
u8 Cycling_Move_right(u16 radius_cm,u16 velocity_cmps)
 {
	//
	u16 distance_cm = (radius_cm * 3.14);
	u16 hori_degree_deg = 0;
	u16 turning_degree_deg = 180;
	u16 deg_velocity_degps = ((180 * velocity_cmps)/ distance_cm);
	if (dt.wait_ck == 0) //没有其他等待校验的CMD时才发送本CMD
	{
		//按协议发送指令
		//左旋
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X02;
		dt.cmd_send.CMD[1] = 0X08;
		dt.cmd_send.CMD[2] = BYTE0(turning_degree_deg);//转180度
		dt.cmd_send.CMD[3] = BYTE1(turning_degree_deg);
		dt.cmd_send.CMD[4] = BYTE0(deg_velocity_degps);//速度 cmps 15better
		dt.cmd_send.CMD[5] = BYTE1(deg_velocity_degps);
		CMD_Send(0xff, &dt.cmd_send);
		//平移
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X02;
		dt.cmd_send.CMD[1] = 0X03;
		//
		dt.cmd_send.CMD[2] = BYTE0(distance_cm);
		dt.cmd_send.CMD[3] = BYTE1(distance_cm);
		dt.cmd_send.CMD[4] = BYTE0(velocity_cmps);
		dt.cmd_send.CMD[5] = BYTE1(velocity_cmps);
		dt.cmd_send.CMD[6] = BYTE0(hori_degree_deg);
		dt.cmd_send.CMD[7] = BYTE1(hori_degree_deg);
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}